use core::{ffi::c_void, marker::PhantomData, ptr::null_mut};

use crate::{
    c::rtx_os::{
        osMessageQueueAttr_t, osMessageQueueGet, osMessageQueueId_t, osMessageQueueNew,
        osMessageQueuePut, osMessageQueueReset, osRtxMessageQueue_t,
    },
    error::{parse_error, Error},
    utils::AlignStore,
};

const CB_SIZE: usize = size_of::<osRtxMessageQueue_t>();

pub struct MessageQueue<T, const COUNT: usize, const MQ_SIZE: usize> {
    cb_mem: AlignStore<CB_SIZE>,
    mq_mem: AlignStore<MQ_SIZE>,
    id: osMessageQueueId_t,
    _p: PhantomData<T>,
}

/**
 * /// Memory size in bytes for Message Queue storage.
/// \param         msg_count     maximum number of messages in queue.
/// \param         msg_size      maximum message size in bytes.
#define osRtxMessageQueueMemSize(msg_count, msg_size) \
  (4*(msg_count)*(3+(((msg_size)+3)/4)))
 */

pub const fn mq_size<T>(count: usize) -> usize {
    4 * count * (3 + ((size_of::<T>() + 3) / 4))
}

impl<T: Default, const COUNT: usize, const MQ_SIZE: usize> MessageQueue<T, COUNT, MQ_SIZE> {
    pub const fn default() -> Self {
        Self {
            cb_mem: AlignStore::new(),
            mq_mem: AlignStore::new(),
            id: null_mut(),
            _p: PhantomData,
        }
    }

    pub unsafe fn new(&mut self, name: &str) {
        let attr = osMessageQueueAttr_t {
            name: name.as_ptr() as *const i8,
            attr_bits: 0,
            cb_mem: self.cb_mem.raw_ptr(),
            cb_size: CB_SIZE as u32,
            mq_mem: self.mq_mem.raw_ptr(),
            mq_size: MQ_SIZE as u32,
        };
        self.id = osMessageQueueNew(COUNT as u32, size_of::<T>() as u32, &attr);
        assert!(self.id != null_mut(), "message queue create fail");
    }

    pub fn post(&self, v: &T) -> Result<(), Error> {
        let ret = unsafe { osMessageQueuePut(self.id, v as *const T as *const c_void, 0, 0) };
        parse_error((), ret)
    }

    pub fn poll(&self, timeout: u32) -> Result<T, Error> {
        let v = T::default();
        let ret = unsafe {
            osMessageQueueGet(
                self.id,
                (&v as *const T) as *mut c_void,
                null_mut(),
                timeout,
            )
        };
        parse_error(v, ret)
    }

    pub fn reset(&self) -> Result<(), Error> {
        let ret = unsafe { osMessageQueueReset(self.id) };
        parse_error((), ret)
    }
}
